# from dijkstar import fordijkstar
import numpy as np
import operator
# import numba as nb
import math

# dijkstar
# @nb.jit(nopython=True)
# def new_for(value):
#     for_find_car = []
#     for i in range(len(value)):
#         for_find_car.append(value[i][0])
#     return for_find_car

# @nb.jit(nopython=True)

def Astar_new(Car, Cross, Road, PositionData, car_chromo,vlist_table):
    car_chromo = car_chromo[0,:]
    # car_chromo[2] = 1
    # car_chromo[3] = 2
    car_number = car_chromo[0]
    # car_chromo[6] = 2
    # 找到car_number当前的路口号，和从哪个路口来的
    # max_car_chromo 表示车走到的当前最大步数

    max_car_chromo = max(car_chromo[2:])
    if max_car_chromo == 0:
        current_road_number = Car[int(np.argwhere(Car[:, 0] == car_number)), 1]
    else:
        current_road_number = int(np.argwhere(car_chromo[2:]==max_car_chromo)) + 1
    # current_road_number = int(np.argwhere(car_chromo[2:]==max_car_chromo)) + 1
    # current_road_number = car_chromo[2:].index(max_car_chromo) + 1
    if max_car_chromo == 0 or max_car_chromo == 1:
        pre_road_number = current_road_number
    else:
        pre_road_number = int(np.argwhere(car_chromo[2:]==max_car_chromo-1)) + 1

    car_index = int(np.argwhere(Car[:, 0] == car_number))
    end_road_number = int(Car[car_index][2])

    def find_currentNode(node_all):
        temp_node_dic = {}
        for temp_node in node_all.keys():
            temp_node_value = node_all[temp_node]
            temp_node_dic[temp_node] = temp_node_value[1] + temp_node_value[2]
        currentNode1 = sorted(temp_node_dic.items(), key=operator.itemgetter(1))
        currentNode = []
        for i in range(len(currentNode1)):
            currentNode.append(currentNode1[i][0])
        return currentNode

    class AStar:
        """
        AStar算法的Python3.x实现
        """

        class Node:  # 描述AStar算法中的节点数据
            def __init__(self, point, endPoint, g=0):
                self.point = point  # 自己的坐标,当前路口
                # self.father = None  # 父节点
                # self.g = g  # g值，g值放路况
                next_point = []
                current_Cross_index = int(np.argwhere(Cross[:, 0] == point))
                for i in range(4):
                    if Cross[current_Cross_index][i+1] == -1:
                        continue
                    else:
                        # temp_road_number = Cross[current_Cross_index][i+1]
                        # temp_road_index = int(np.argwhere(Road[:, 0] == temp_road_number))
                        temp_road_index = int(np.argwhere(Road[:, 0] == Cross[current_Cross_index][i+1]))
                        if Road[temp_road_index][4] != current_road_number and Road[temp_road_index][4] != pre_road_number and Road[temp_road_index][6] == 1:
                            next_point.append([Road[temp_road_index][4],temp_road_index])
                        if Road[temp_road_index][5] != current_road_number and Road[temp_road_index][5] != pre_road_number:
                            next_point.append([Road[temp_road_index][5],temp_road_index])
                self.openList = next_point   # 接下来的路口,已经删除上一个路口,两个值，一个路口号，一个是该路口号与车辆所在位置路口的之间的道路编号在Road中的index
                self.closeList = pre_road_number  # 走过的路口

        def __init__(self, startPoint, endPoint, passTag=0):
            """
            构造AStar算法的启动条件
            :param map2d: Array2D类型的寻路数组
            :param startPoint: Point或二元组类型的寻路起点
            :param endPoint: Point或二元组类型的寻路终点
            :param passTag: int类型的可行走标记（若地图数据!=passTag即为障碍）
            """
            # 开启表
            self.openList = []
            # 关闭表
            self.closeList = []
            # 道路容量
            self.road_capacity = []
            self.startPoint = startPoint
            self.endPoint = endPoint

        def getMinNode(self):
            """
            获得openlist中F值最小的节点
            :return: Node
            """

            node_all = {}
            temp_node_all = {}
            # print("self.openList[0].openList")
            # print(self.openList[0].openList)
            for node in self.openList[0].openList:
                node_road_number_index = node[1]
                # 找到道路编号node_road_number
                if current_road_number < node[0]:
                    node_PositionData = PositionData[node_road_number_index, 0:Road[node_road_number_index][3], :]
                else:
                    node_PositionData = PositionData[node_road_number_index, Road[node_road_number_index][3]:, :]
                # 道路容量
                # road_capacity = np.sum(node_PositionData == 0) / (
                #             Road[node_road_number_index][3] * Road[node_road_number_index][1])
                road_capacity = np.sum(node_PositionData == 0)
                value_h1 = vlist_table[node[0]][self.endPoint].dist
                temp_node_all[node[0]] = [road_capacity, abs(Car[car_index][3] - Road[node_road_number_index][2]),
                                          value_h1]
                # value_h2 = Road[node_road_number_index][1] / min(Car[car_index][3], Road[node_road_number_index][2])
                if road_capacity > 0:   # 有一个空位就不上去了，防止死锁
                    # value_g = abs(Car[car_index][3] - Road[node_road_number_index][2]) + Road[node_road_number_index][
                    #     3] + math.pow(1-road_capacity, 1)
                    # value_g = abs(Car[car_index][3] - Road[node_road_number_index][2]) #+ 10*math.pow(1 - road_capacity, 1)
                    # 字典node_all中外面的关键字是路口号，里面的值是车道容量，原始G和H
                    node_all[node[0]] = [road_capacity, abs(Car[car_index][3] - Road[node_road_number_index][2]), value_h1]
            # print("temp_node_all")
            # print(temp_node_all.keys())

            if len(node_all) == 0 and current_road_number == pre_road_number:
                sort_node = [-1, -1, -1, -1]
                return sort_node

            if len(node_all) == 0 or len(node_all) == len(temp_node_all):
                if len(temp_node_all) == 1:
                    sort_node = list(temp_node_all.keys())
                else:
                    sort_node = find_currentNode(temp_node_all)
                # currentNode = -1
                # sort_node = find_currentNode(temp_node_all)
                # hahah = 11
                # return find_currentNode(temp_node_all)
            if len(node_all) == 1 and len(temp_node_all) > 1:
                sort_node = list(node_all.keys())
                del temp_node_all[sort_node[0]]
                sort_node.append(find_currentNode(temp_node_all))
                # return int(currentNode[0])
            if len(node_all) == len(temp_node_all):
                sort_node = list(node_all.keys())
                # del temp_node_all[sort_node[0]]
                # sort_node.append(find_currentNode(temp_node_all))
            if len(node_all) != len(temp_node_all) and len(node_all) > 1:
                sort_node = find_currentNode(node_all)
                for i in range(len(sort_node)):
                    del temp_node_all[sort_node[i]]
                if len(temp_node_all) == 1:
                    sort_node.append(int(temp_node_all.keys()))
                else:
                    sort_node.append(find_currentNode(temp_node_all))

            # if len(node_all) == 0 or len(node_all) == len(temp_node_all):
            #     # currentNode = -1
            #     sort_node = find_currentNode(temp_node_all)
            #     # hahah = 11
            #     # return find_currentNode(temp_node_all)
            # if len(node_all) == 1 and len(temp_node_all) > 1:
            #     sort_node = list(node_all.keys())
            #     del temp_node_all[sort_node[0]]
            #     sort_node.append(find_currentNode(temp_node_all))
            #     # return int(currentNode[0])
            # if len(node_all) == 1 and len(temp_node_all) == 1:
            #     sort_node = list(node_all.keys())
            #     del temp_node_all[sort_node[0]]
            #     sort_node.append(find_currentNode(temp_node_all))
            # if len(node_all) != len(temp_node_all) and len(node_all) > 1:
            #     sort_node = find_currentNode(node_all)
            #     for i in range(len(sort_node)):
            #         del temp_node_all[sort_node[i]]
            #     if len(temp_node_all) == 1:
            #         sort_node.append(int(temp_node_all.keys()))
            #     else:
            #         sort_node.append(find_currentNode(temp_node_all))
                # return find_currentNode(node_all)
            # justhahh = 1
            return sort_node

        def start(self):
            """
            开始寻路
            :return: None或Point列表（路径）
            """
            # # 判断寻路终点是否是障碍
            # if self.map2d[self.endPoint.x][self.endPoint.y] != self.passTag:
            #     return None

            # 1.将起点放入开启列表
            startNode = AStar.Node(self.startPoint, self.endPoint)
            self.openList.append(startNode)
            # 找到F值最小的点
            minF = self.getMinNode()
            return minF
            # # 2.主循环逻辑
            # while True:
            #     # 找到F值最小的点
            #     minF = self.getMinNode()
            #     # 把这个点加入closeList中，并且在openList中删除它
            #     return minF

    # 创建AStar对象,并设置起点为当前路口 终点为end_road_number  是路口号
    aStar = AStar(current_road_number, end_road_number)
    # 开始寻路
    nextpath = aStar.start()
    nextpath_index = []
    # print(nextpath)
    for i in range(len(nextpath)):
        # print(nextpath[i])
        # a = 0
        # a = nextpath[i]-1
        nextpath_index.append(nextpath[i]-1)
    Car[car_index][12:12+len(nextpath)] = nextpath_index
    # car_chromo[nextpath + 1] = max_car_chromo + 1
    return Car